#include "Matrix3.h"

const Matrix3<float> Matrix3<float>::ZERO ( 0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f );
const Matrix3<float> Matrix3<float>::IDENTITY (1.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,1.0f);
const float Matrix3<float>::epsilon = 1e-04f;
const int Matrix3<float>::max_iterations = 32;

template <class Real>
Matrix3<Real>::Matrix3(const Real entry[3][3] )
{
	memcpy( m_entry, entry, 9*sizeof(entry[0][0]) );
}

template <class Real>
Matrix3<Real>::Matrix3(const Matrix<3,Real>& matrix )
{
	memcpy( m_entry, matrix.m_entry, 9*sizeof(Real) );
}

template <class Real>
Matrix3<Real>::Matrix3( Real f0, Real f1, Real f2, Real f3,
				 Real f4, Real f5, Real f6, Real f7,
				 Real f8 )
{
	entries[0] = f0;
	entries[1] = f1;
	entries[2] = f2;
	entries[3] = f3;
	entries[4] = f4;
	entries[5] = f5;
	entries[6] = f6;
	entries[7] = f7;
	entries[8] = f8;
}

template <class Real>
Matrix3<Real>::Matrix3 ( const Real arraym[9], bool rowmajor )
{
	if ( rowmajor )
	{
		memcpy( entries, arraym, 9 * sizeof( Real ) ); 
	}
	else
	{
		entries[0] = arraym[0];
		entries[1] = arraym[3];
		entries[2] = arraym[6];
		entries[3] = arraym[1];
		entries[4] = arraym[4];
		entries[5] = arraym[7];
		entries[6] = arraym[2];
		entries[7] = arraym[5];
		entries[8] = arraym[8];
	}
}

template <class Real>
Matrix3<Real>::Matrix3( const Vector3<Real>& u,
					   const Vector3<Real>& v, const Vector3<Real>& w, bool columns )
{
	if ( columns )
	{
		entries[0] = u[0];
		entries[1] = v[0];
		entries[2] = w[0];
		entries[3] = u[1];
		entries[4] = v[1];
		entries[5] = w[1];
		entries[6] = u[2];
		entries[7] = v[2];
		entries[8] = w[2];
	}
	else
	{
		entries[0] = u[0];
		entries[1] = u[1];
		entries[2] = u[2];
		entries[3] = v[0];
		entries[4] = v[1];
		entries[5] = v[2];
		entries[6] = w[0];
		entries[7] = w[1];
		entries[8] = w[2];
	}
}

template <class Real>
Matrix3<Real>::Matrix3( const Vector3<Real>* v, bool columns )
{
	if ( columns )
	{
		entries[0] = v[0][0];
		entries[1] = v[1][0];
		entries[2] = v[2][0];
		entries[3] = v[0][1];
		entries[4] = v[1][1];
		entries[5] = v[2][1];
		entries[6] = v[0][2];
		entries[7] = v[1][2];
		entries[8] = v[2][2];
	}
	else
	{
		entries[0] = v[0][0];
		entries[1] = v[0][1];
		entries[2] = v[0][2];
		entries[3] = v[1][0];
		entries[4] = v[1][1];
		entries[5] = v[1][2];
		entries[6] = v[2][0];
		entries[7] = v[2][1];
		entries[8] = v[2][2];
	}
}

template <class Real>
Matrix3<Real>::Matrix3(	const Vector3<Real>& u, const Vector3<Real>& v )
{
	MakeTensorProduct( u, v );
}

template <class Real>
void Matrix3<Real>::MakeTensorProduct( const Vector3<Real>& u, const Vector3<Real>& v )
{
	entries[0] = u[0]*v[0];
	entries[1] = u[0]*v[1];
	entries[2] = u[0]*v[2];
	entries[3] = u[1]*v[0];
	entries[4] = u[1]*v[1];
	entries[5] = u[1]*v[2];
	entries[6] = u[2]*v[0];
	entries[7] = u[2]*v[1];
	entries[8] = u[2]*v[2];
}

template <class Real>
Matrix3<Real>::Matrix3( Real m00, Real m11, Real m22 )
{
	MakeDiagonal( m00, m11, m22 );
}

template <class Real>
void Matrix3<Real>::MakeDiagonal( Real m00, Real m11, Real m22 )
{
	entries[0] = m00;
	entries[1] = (Real) 0.0;
	entries[2] = (Real) 0.0;
	entries[3] = (Real) 0.0;
	entries[4] = m11;
	entries[5] = (Real) 0.0;
	entries[6] = (Real) 0.0;
	entries[7] = (Real) 0.0;
	entries[8] = m22;
}


template <class Real>
Matrix3<Real>::Matrix3( const Vector3<Real>& axis, Real angle )
{
	FromAxisAngle( axis, angle );
}

template <class Real>
void Matrix3<Real>::FromAxisAngle( const Vector3<Real>& axis, Real angle )
{
	Real cs = Math<Real>::cos_t( angle );
	Real sn = Math<Real>::sin_t( angle );
	Real oneminus = ((Real) 1.0 ) - cs;
	Real fx2 = axis[0] * axis[0];
	Real fy2 = axis[1] * axis[1];
	Real fz2 = axis[2] * axis[2];
	Real fxym = axis[0] * axis[1] * oneminus;
	Real fxzm = axis[0] * axis[2] * oneminus;
	Real fyzm = axis[1] * axis[2] * oneminus;
	Real xsin = axis[0] * sn;
	Real ysin = axis[1] * sn;
	Real zsin = axis[2] * sn;

	entries[0] = fx2 * oneminus + cs;
	entries[1] = fxym - zsin;
	entries[2] = fxzm + ysin;
	entries[3] = fxym + zsin;
	entries[4] = fy2 * oneminus + cs;
	entries[5] = fyzm - xsin;
	entries[6] = fxzm - ysin;
	entries[7] = fyzm + xsin;
	entries[8] = fz2 * oneminus + cs;
}


template <class Real>
Matrix3<Real> Matrix3<Real>::Inverse () const
{
	Matrix3<Real> inverse;

	inverse[0][0] = entries[4]*entries[8] - entries[5]*entries[7];
	inverse[0][1] = entries[2]*entries[7] - entries[1]*entries[8];
	inverse[0][2] = entries[1]*entries[5] - entries[2]*entries[4];
	inverse[1][0] = entries[5]*entries[6] - entries[3]*entries[8];
	inverse[1][1] = entries[0]*entries[8] - entries[2]*entries[6];
	inverse[1][2] = entries[2]*entries[3] - entries[0]*entries[5];
	inverse[2][0] = entries[3]*entries[7] - entries[4]*entries[6];
	inverse[2][1] = entries[1]*entries[6] - entries[0]*entries[7];
	inverse[2][2] = entries[0]*entries[4] - entries[1]*entries[3];

	Real det = entries[0]*inverse[0][0] + entries[1]*inverse[1][0] +
		entries[2]*inverse[2][0];

	if ( Math<Real>::abs(det) <= Math<Real>::EPSILON )
		return Matrix3::ZERO;

	inverse /= det;
	return inverse;
}

template <class Real>
Matrix3<Real> Matrix3<Real>::Adjoint() const
{
	Matrix3<Real> adjoint;
	
	adjoint[0][0] = entries[4]*entries[8] - entries[5]*entries[7];
	adjoint[0][1] = entries[2]*entries[7] - entries[1]*entries[8];
	adjoint[0][2] = entries[1]*entries[5] - entries[2]*entries[4];
	adjoint[1][0] = entries[5]*entries[6] - entries[3]*entries[8];
	adjoint[1][1] = entries[0]*entries[8] - entries[2]*entries[6];
	adjoint[1][2] = entries[2]*entries[3] - entries[0]*entries[5];
	adjoint[2][0] = entries[3]*entries[7] - entries[4]*entries[6];
	adjoint[2][1] = entries[1]*entries[6] - entries[0]*entries[7];
	adjoint[2][2] = entries[0]*entries[4] - entries[1]*entries[3];

	return adjoint;
}

template <class Real>
Real Matrix3<Real>::Determinant () const
{
	Real co00 = entries[4]*entries[8] - entries[5]*entries[7];
	Real co10 = entries[5]*entries[6] - entries[3]*entries[8];
	Real co20 = entries[3]*entries[7] - entries[4]*entries[6];
	Real det  = entries[0]*co00 + entries[1]*co10 + entries[2]*co20;
	return det;
}

template <class Real>
void Matrix3<Real>::ToAxisAngle( Vector3<Real>& axis, Real& angle ) const
{
	Real trace = entries[0] + entries[4] + entries[8];
	Real cs = ((Real) 0.5)*(trace - ((Real)1.0));
	angle = Math<Real>::cos_t(cs);

	if ( angle > (Real) 0.0 )
	{
		if ( angle < Math<Real>::PI )
		{
			axis[0] = entries[7] - entries[5];
			axis[1] = entries[2] - entries[6];
			axis[2] = entries[3] - entries[1];
			axis.Normalize();
		}
		else
		{
			// angle is PI
			Real halfinverse;
			if ( entries[0] >= entries[4] )
			{
				if ( entries[0] >= entries[8] )
				{
					axis[0] = ((Real)0.5) * Math<Real>::sqrt_t(entries[0] - 
						entries[4] - entries[8] + (Real) 1.0 );
					halfinverse = ((Real)0.5)/axis[0]);
					axis[1] = halfinverse * entries[1];
					axis[2] = halfinverse * entries[2];
				}
				else
				{
					// r22 is maximum diagonal term
					axis[2] = ((Real)0.5)*Math<Real>::sqrt_t( entries[8] - 
						entries[0] - entries[4] + (Real) 1.0 );
					halfinverse = ((Real)0.5) / axis[2];
					axis[0] = halfinverse * entries[2];
					axis[1] = halfinverse * entries[5];
				}
			}
			else
			{
				// r11 > r00
				if ( entries[4] >= entries[8] )
				{
					// r11 is max diagonal term
					axis[1] = ((Real)0.5) * Math<Real>::sqrt_t( entries[4] - 
						entries[0] - entries[8] + (Real) 1.0 );
					halfinverse = ((Real) 0.5) / axis[1];
					axis[0] = halfinverse * entries[1];
					axis[2] = halfinverse * entries[5];
				}
				else
				{
					// r22 is max diagonal term
					axis[2] = ((Real)0.5)*Math<Real>::sqrt_t( entries[8] - 
						entries[0] - entries[4] + (Real)1.0 );
					halfinverse = ((Real)0.5) / axis[2];
					axis[0] = halfinverse * entries[2];
					axis[1] = halfinverse * entries[5];
				}
			}
		}
	}
	else
	{
		// The angle is 0 and the matrix is the identity. Any axis will
		// work, so just use the x-axis.
		axis[0] = (Real) 1.0;
		axis[1] = (Real) 0.0;
		axis[2] = (Real) 0.0;
	}
}

template <class Real>
void Matrix3<Real>::Orthonormalize()
{
	// compute q0
	Real invlength = Math<Real>::invsqrt( entries[0] * entries[0] +
		entries[3]*entries[3] + entries[6]*entries[6] );
	// compute q1
	Real dot0 = entries[0]*entries[1] + entries[3]*entries[4] +
		entries[6]*entries[7];

	entries[1] -= dot0*entries[0];
	entries[4] -= dot0*entries[3];
	entries[7] -= dot0*entries[6];

	invlength = Math<Real>::invsqrt(entries[1]*entries[1] + 
		entries[4]*entries[4] + entries[7]*entries[7] );

	// compute q2
	Real dot1 = entries[1]*entries[2] + entries[4]*entries[5] +
		entries[7]*entries[8];

	Real dot0 = entries[0]*entries[2] + entries[3]*entries[5] +
		entries[6]*entries[8];

	entries[2] -= dot0*entries[0] + dot1*entries[1];
	entries[5] -= dot0*entries[3] + dot1*entries[4];
	entries[8] -= dot0*entries[6] + dot1*entries[7];

	invlength = Math<Real>::invsqrt(entries[2]*entries[2] + 
		entries[5]*entries[5] + entries[8]*entries[8] );

	entries[2] *= invlength;
	entries[5] *= invlength;
	entries[8] *= invlength;
}

template <class Real>
bool Matrix3<Real>::ToEulerAnglesXYZ( Real& xangle, Real& yangle, Real& zangle ) const
{
	if ( entries[2] < (Real)1.0 )
	{
		if ( entries[2] > -(Real) 1.0 )
		{
			xangle = Math<Real>::atan2_t(-entries[5], entries[8]);
			yangle = Math<Real>::asin_t((double) entries[2] );
			zangle = Math<Real>::atan2_t( -entries[1], entries[0] );
			return true;
		}
		else
		{
			xangle = -Math<Real>::atan2_t( entries[3], entries[4] );
			yangle = -Math<Real>::HALF_PI;
			zangle = (Real) 0.0;
			return false;
		}
	}
	else
	{
		xangle = Math<Real>::atan2_t( entries[3], entries[4] );
		yangle = Math<Real>::HALF_PI;
		zangle = (Real)0.0;
		return false;
	}
}

template <class Real>
bool Matrix3<Real>::ToEulerAnglesXZY( Real& xangle, Real& zangle,
									 Real& yangle ) const
{
	if ( entries[1] < (Real)1.0 )
	{
		if ( entries[1] > -(Real)1.0 )
		{
			xangle = Math<Real>::atan2_t( entries[7], entries[4] );
			zangle = Math<Real>::asin_t( -entries[1] );
			yangle = Math<Real>::atan2_t( entries[2], entries[0] );
			return true;
		}
		else
		{
			xangle = Math<Real>::atan2_t( entries[6], entries[8] );
			zangle = Math<Real>::HALF_PI;
			yangle = (Real)0.0;
			return false;
		}
	}
	else
	{
		xangle = Math<Real>::atan2_t( -entries[6], entries[8] );
		zangle = -Math<Real>::HALF_PI;
		yangle = (Real)0.0;
		return false;
	}
}

template <class Real>
bool Matrix3<Real>::ToEulerAnglesYXZ( Real& yangle, Real& xangle, Real& zangle ) const
{
	if ( entries[5] < (Real)1.0 )
	{
		if ( entries[5] > -(Real)1.0 )
		{
			yangle = Math<Real>::atan2_t( entries[2], entries[8] );
			xangle = Math<Real>::asin_t(-(double) entries[5] );
			zangle = Math<Real>::atan2_t( entries[3], entries[4] );
			return true;
		}
		else
		{
			yangle = Math<Real>::atan2_t( entries[1], entries[0] );
			xangle = Math<Real>::HALF_PI;
			zangle = (Real)0.0;
			return false;
		}
	}
	else
	{
		yangle = Math<Real>::atan2_t( -entries[1], entries[0] );
		xangle = -Math<Real>::HALF_PI;
		zangle = (Real)0.0;
		return false;
	}
}

template <class Real>
bool Matrix3<Real>::ToEulerAnglesYZX( Real& yangle, Real& zangle, Real& xangle ) const
{
	if ( entries[3] < (Real)1.0 )
	{
		if ( entries[3] > -(Real)1.0 )
		{
			yangle = Math<Real>::atan2_t( -entries[6], entries[0] );
			zangle = Math<Real>::asin_t( entries[3] );
			xangle = Math<Real>::atan2_t( -entries[5], entries[4] );
			return true;
		}
		else
		{
			yangle = -Math<Real>::atan2_t( entries[7], entries[8] );
			zangle = -Math<Real>::HALF_PI;
			xangle = (Real)0.0;
			return false;
		}
	}
	else
	{
		yangle = Math<Real>::atan2_t( entries[7], entries[8] );
		zangle = Math<Real>::HALF_PI;
		xangle = (Real)0.0;
		return false;
	}
}

template <class Real>
bool Matrix3<Real>::ToEulerAnglesZXY( Real& zangle, Real& xangle, Real& yangle ) const
{
	if ( entries[7] < (Real)1.0 )
	{
		if ( entries[7] > -(Real) 1.0 )
		{
			zangle = Math<Real>::atan2_t( -entries[1], entries[4] );
			xangle = -Math<Real>::asin_t( entries[7] );
			yangle = Math<Real>::atan2_t( -entries[6], entries[8] );
			return true;
		}
		else
		{
			zangle = -Math<Real>::atan2_t( entries[2], entries[0] );
			xangle = -Math<Real>::HALF_PI;
			yangle = (Real)0.0;
			return false;
		}
	}
	else
	{
		zangle = Math<Real>::atan2_t( entries[2], entries[0] );
		xangle = Math<Real>::HALF_PI;
		yangle = (Real)0.0;
	}
}

template <class Real>
bool Matrix3<Real>::ToEulerAnglesZYX( Real& zangle, Real& yangle, Real& xangle ) const
{
	if ( entries[6] < (Real)1.0 )
	{
		if ( entries[6] > -(Real)1.0 )
		{
			zangle = Math<Real>::atan2_t( entries[3], entries[0] );
			yangle = Math<Real>::asin_t(-entries[6]);
			xangle = Math<Real>::atan2_t( entries[7], entries[8] );
			return true;
		}
		else
		{
			zangle = Math<Real>::atan2_t( entries[1], entries[2] );
			yangle = Math<Real>::HALF_PI;
			xangle = (Real)0.0;
			return false;
		}
	}
	else
	{
		zangle = Math<Real>::atan2_t( -entries[1], -entries[2] );
		yangle = -Math<Real>::HALF_PI;
		xangle = (Real)0.0;
		return false;
	}
}

template <class Real>
void Matrix3<Real>::FromEulerAnglesXYZ( Real yangle, Real pangle, Real rangle )
{
	Real cs, sn;

	cs = Math<Real>::cos_t( yangle );
	sn = Math<Real>::sin_t( yangle );
	Matrix3 xmat( (Real) 1.0, (Real) 0.0, (Real) 0.0,
				 (Real) 0.0, cs, -sn,
				 (Real) 0.0, sn, cs );

	cs = Math<Real>::cos_t( pangle );
	sn = Math<Real>::sin_t( pangle );
	Matrix3 ymat( cs, (Real) 0.0, sn,
				(Real) 0.0, (Real)1.0, (Real) 0.0,
				-sn, (Real) 0.0, cs );

	cs = Math<Real>::cos_t( rangle );
	sn = Math<Real>::sin_t( rangle );
	Matrix3 zmat(
		cs, -sn, (Real) 0.0,
		sn, cs, (Real) 0.0,
		(Real) 0.0, (Real) 0.0, (Real)1.0 );

	*this = xmat * ( ymat * zmat );
}

template <class Real>
void Matrix3<Real>::FromEulerAnglesXZY( Real yangle, Real pangle, Real rangle )
{
	Real cs, sn;

	cs = Math<Real>::cos_t( yangle );
	sn = Math<Real>::sin_t( yangle );

	Matrix3 xmat( 
		(Real) 1.0, (Real) 0.0, (Real) 0.0,
		(Real) 0.0, cs        , -sn       ,
		(Real) 0.0, sn        ,  cs       );

	cs = Math<Real>::cos_t( pangle );
	sn = Math<Real>::sin_t( pangle );
	Matrix3 zmat(
		cs, -sn, (Real) 0.0,
		sn,  cs, (Real) 0.0,
		(Real) 0.0, (Real)0.0, (Real)1.0 );

	cs = Math<Real>::cos_t( rangle );
	sn = Math<Real>::sin_t( rangle );
	Matrix3 ymat( 
		cs, (Real)0.0, sn,
		(Real)0.0, (Real)1.0, (Real)0.0,
		-sn, (Real) 0.0, cs );

	*this = xmat * ( zmat * ymat );
}

template <class Real>
void Matrix3<Real>::FromEulerAnglesYXZ( Real yangle, Real pangle, Real rangle )
{
	Real cs, sn;

	cs = Math<Real>::cos_t( yangle );
	sn = Math<Real>::sin_t( yangle );
	Matrix3 ymat(
		 cs       , (Real) 0.0, sn        ,
		(Real) 0.0, (Real) 1.0, (Real) 0.0,
		-sn       , (Real) 0.0,        cs );

	cs = Math<Real>::cos_t( pangle );
	sn = Math<Real>::sin_t( pangle );
	Matrix3 xmat(
		(Real) 1.0, (Real) 0.0, (Real) 0.0,
		(Real) 0.0,         cs,        -sn,
		(Real) 0.0,         sn,         cs );
	
	cs = Math<Real>::cos_t( rangle );
	sn = Math<Real>::sin_t( rangle );
	Matrix3 zmat(
		cs        ,        -sn, (Real) 0.0,
		sn        ,         cs, (Real) 0.0,
		(Real) 0.0, (Real) 0.0, (Real) 1.0 );

	*this = xmat * ( ymat * zmat );
}

template <class Real>
Matrix3<Real> Matrix3<Real>::Slerp( Real t, const Matrix3& r0,
								   const Matrix3& r1 )
{
	Vector3<Real> axis;
	Real angle;
	Matrix prod = r0.TransposeTimes( r1 );
	prod.ToAxisAngle( axis, angle );
	return Matrix3( axis, t*angle );
}

template <class Real>
void Matrix3<Real>::Tridiagonalize( Real diag[3], Real subdiag[3] )
{
	Real a = entries[0];
	Real b = entries[1];
	Real c = entries[2];
	Real d = entries[4];
	Real e = entries[5];
	Real f = entries[8];

	diag[0] = a;
	subdiag[2] = (Real)0.0;
	if ( Math<Real>::abs( c ) >= Math<Real>::EPSILON )
	{
		Real length = Math<Real>::sqrt_t( b*b+c*c );
		Real invlength = ((Real)1.0)/length;
		b *= invlength;
		c *= invlength;
		Real q = ((Real)2.0)*b*e+c*(f-d);
		diag[1] = d+c*q;
		diag[2] = f-c*q;
		subdiag[0] = length;
		subdiag[1] = e-b*q;

		entries[0] = (Real) 1.0;
		entries[1] = (Real) 0.0;
		entries[2] = (Real) 0.0;
		entries[3] = (Real) 0.0;
		entries[4] = b;
		entries[5] = c;
		entries[6] = (Real) 0.0;
		entries[7] = c;
		entries[8] = -b;
	}
	else
	{
		diag[1] = d;
		diag[2] = f;
		subdiag[0] = b;
		subdiag[1] = e;

		entries[0] = (Real) 1.0;
		entries[1] = (Real) 0.0;
		entries[2] = (Real) 0.0;
		entries[3] = (Real) 0.0;
		entries[4] = (Real) 1.0;
		entries[5] = (Real) 0.0;
		entries[6] = (Real) 0.0;
		entries[7] = (Real) 0.0;
		entries[8] = -(Real) 1.0;
	}
}

template <class Real>
Matrix3<Real>& Matrix3<Real>::operator= ( const Matrix3& matrix )
{
	memcpy( entries, matrix.entries, 9*sizeof(Real) );
	return *this;
}


/*

bool Matrix3::operator== (const Matrix3& matrix ) const
{
	for ( int row = 0; row < 3; row++ )
	{
		for ( int col = 0; col < 3; col++ )
		{
			if ( m_entry[row][col] != matrix.m_entry[row][col] )
				return false;
		}
	}
	return true;
}

bool Matrix3::operator!= (const Matrix3& matrix ) const
{
	return !operator==(matrix);
}

Matrix3 Matrix3::operator+ ( const Matrix3& matrix ) const
{
	Matrix3 sum;

	for ( int i = 0; i < 3; i++ )
	{
		for ( int j = 0; j < 3; j++ )
		{
			sum.m_entry[i][j] = m_entry[i][j] + 
				matrix.m_entry[i][j];
		}
	}
	return sum;
}

Matrix3 Matrix3::operator- ( const Matrix3& matrix ) const
{
	Matrix3 diff;

	for ( int i = 0; i < 3; i++ )
	{
		for ( int j = 0; j < 3; j++ )
		{
			diff.m_entry[i][j] = m_entry[i][j] - 
				matrix.m_entry[i][j];
		}
	}
	return diff;
}

Matrix3 Matrix3::operator* ( const Matrix3& matrix ) const
{
	Matrix3 prod;

	for ( int i = 0; i < 3; i++ )
	{
		for ( int j = 0; j < 3; j++ )
		{
			prod.m_entry[i][j] = m_entry[i][0] * matrix.m_entry[0][j] +
								 m_entry[i][1] * matrix.m_entry[1][j] + 
								 m_entry[i][2] * matrix.m_entry[2][j];
		}
	}
	return prod;
}

Vector3 Matrix3::operator* ( const Vector3& point ) const
{
	Vector3 prod;
	for ( int i = 0; i < 3; i++ )
	{
		prod[i] = 
			m_entry[i][0] * point[0] +
			m_entry[i][1] * point[1] +
			m_entry[i][2] * point[2];
	}
	return prod;
}


Vector3 operator* (const Vector3& point, const Matrix3& matrix )
{
	Vector3 prod;

	for ( int i = 0; i < 3; i++ )
	{
		prod[i] =
			point[0] * matrix.m_entry[0][i]
*/